//
// Created by cyt on 2023/9/4.
//

#ifndef ICM42688_BASE_H
#define ICM42688_BASE_H

#include <stdint.h>

#ifdef __cplusplus
extern "C"
{
#endif

#include "Invn/Drivers/Icm426xx/Icm426xxTransport.h"
#include "Invn/Drivers/Icm426xx/Icm426xxDefs.h"
#include "Invn/Drivers/Icm426xx/Icm426xxDriver_HL.h"
#include "Invn/Drivers/Icm426xx/Icm426xxSelfTest.h"
#include "Invn/EmbUtils/Message.h"

#include "Invn/LibAGM/invn_algo_agm.h"

#include "system-interface.h"

#include "Invn/Helpers/Icm426xx/helperClockCalib.h"

#ifdef __cplusplus
}
#endif

#define SERIF_TYPE ICM426XX_UI_SPI4
#define RAD_TO_DEG(rad) ((float)rad * 57.2957795131)

typedef struct
{
    float gyro[3]; // x-y-z
    float accel[3];
    float gyro_rad[3];
    int16_t temperature;
} IcmRawData_t;

/* Bias previously stored in flash */
typedef struct sensor_biases
{
    int32_t bias_q16[3];
    uint8_t is_saved;
} sensor_biases_t;

typedef struct
{
    /* data */
    float pitch;
    float yaw;
    float roll;
} EulerAngle_t;

class Icm42688
{
public:
    struct inv_icm426xx_serif icm_serif;
    EulerAngle_t eulerAngle;

    static struct inv_icm426xx icm_driver;
    static IcmRawData_t icm_data;
    static InvnAlgoAGMInput input;
    static InvnAlgoAGMOutput output;
    static clk_calib_t clk_calib;
    static int iter_algo;

    void ConfigInit(int (*read_reg)(struct inv_icm426xx_serif *, uint8_t, uint8_t *,
                                    uint32_t),
                    int (*write_reg)(struct inv_icm426xx_serif *, uint8_t, const uint8_t *,
                                     uint32_t),
                    uint32_t max_read,
                    uint16_t max_write);

    int SetupInvDevice(struct inv_icm426xx_serif *icm_serif, uint8_t _enable_fifo);
    int RunSelfTest(void);
    void GetBias(void);

    /**
     * \brief This function configures the device in order to output gyro and accelerometer.
     *
     * It initialyses clock calibration module (this will allow to extend the 16 bits
     * timestamp produced by Icm426xx to a 64 bits timestamp).
     * Then function sets full scale range and frequency for both accel and gyro and it
     * starts them in the requested power mode.
     *
     * \param[in] is_low_noise_mode : if true sensors are started in low-noise mode else in
     *                                low-power mode.
     * \param[in] acc_fsr_g :   full scale range for accelerometer. See ICM426XX_ACCEL_CONFIG0_FS_SEL_t in Icm426xxDefs.h
     *                         for possible values.
     * \param[in] gyr_fsr_dps : full scale range for gyroscope. See ICM426XX_GYRO_CONFIG0_FS_SEL_t in Icm426xxDefs.h
     *                         for possible values.
     * \param[in] acc_freq :    accelerometer frequency. See ICM426XX_ACCEL_CONFIG0_ODR_t in Icm426xxDefs.h
     *                         for possible values.
     * \param[in] gyr_freq :    gyroscope frequency. See ICM426XX_GYRO_CONFIG0_ODR_t in Icm426xxDefs.h
     *                         for possible values.
     * \param[in] is_rtc_mode :    requested status for RTC/CLKIN feature
     * \return 0 on success, negative value on error.
     */
    int ConfigureInvDevice(uint8_t is_low_noise_mode, int32_t acc_fsr_g,
                           int32_t gyr_fsr_dps,
                           ICM426XX_ACCEL_CONFIG0_ODR_t acc_freq, ICM426XX_GYRO_CONFIG0_ODR_t gyr_freq,
                           uint8_t is_rtc_mode);

    /**
     * \brief This function is the custom handling packet function.
     *
     * It is passed in parameter at driver init time and it is called by
     * inv_icm426xx_get_data_from_registers function each time new data are read from registers.
     * In this implementation, function process data and print them on UART.
     *
     * \param[in] event structure containing sensor data from one packet
     */

    int GetDataFromInvDevice(void);
    int GetDataFromInvDeviceFIFO(void);
    /**
     * \This function clears biases and accuracies.
     *
     * \return 0 on success, negative value on error.
     */
    int ResetInvAGMBiases(void);

    /**
     * \brief This function initializes biases and accuracies for accelerometer, gyroscope and magnetometer.
     *
     * \return 0 on success, negative value on error.
     */
    int InitInvAGMBiases(void);

    /**
     * \brief This function initializes the AGM algorithm.
     *
     * \return 0 on success, negative value on error.
     */
    int InitInvAGMAlgo(void);
    void quaternions_to_angles(const float quat[4]);
    void fixedpoint_to_float(const int32_t *in, float *out, const uint8_t fxp_shift,
                             const uint8_t dim);
#if !IS_HIGH_RES_MODE
    int gyro_fsr_dps_to_bitfield(int32_t fsr);
    int accel_fsr_g_to_bitfield(int32_t fsr);

#endif

#if USE_MAG
    /*!
     * \brief Set up magnetometer AKM09915
     * \param[in] akm_serif : Serial interface object.
     * \return 0 in case of success, negative value in case
     */
    int SetupMagDevice(struct inv_ak0991x_serif *akm_serif);

    /*!
     * \brief Start a new data acquisition on magnetometer.
     *
     * Magnetometer only works in one shot mode. Thus this function only initiates one data
     * acquisition.
     *
     * \return 0 in case of success, negative value in case
     */
    int StartMagDeviceAcquisition(void);

    /*!
     * \brief This function reads data from the Akm09915 and pipe them to the algorithms.
     * Finnally it prints alogrithms output on log uart.
     *
     * \return 0 on success, negative value on error.
     */
    int GetDataFromMagDevice(void);
#endif /* USE_MAG */

    float get_gyr_sensitivity() { return gyr_sensitivity; }
    float get_acc_sensitivity() { return acc_sensitivity; }

private:
    int32_t acc_bias[3];
    int32_t gyr_bias[3];
    int32_t mag_bias[3];
    int32_t acc_accuracy;
    int32_t gyr_accuracy;
    int32_t mag_accuracy;

    int32_t gyr_fsr; // 陀螺仪量程
    int32_t acc_fsr; // 加速度量程

    float acc_sensitivity;
    float gyr_sensitivity;

    uint32_t odr_bitfield_to_us(uint32_t odr_bitfield);
    void set_acc_sensitivity();
    void set_gyro_snesitivity();

    // 回调函数处理部分
    static void HandleInvDeviceDataRegisters(inv_icm426xx_sensor_event_t *event);
    static void HandleInvDeviceFifoPacket(inv_icm426xx_sensor_event_t *event);

    // 辅助函数部分
    static void apply_mounting_matrix(const int32_t matrix[9], int16_t raw[3]);
    static void apply_mounting_matrix_32(const int32_t matrix[9], int32_t raw[3]);

    /*
     * \brief Evaluate wether biases needs to be written to flash depending on accuracies value
     */
    // Flash辅助处理函数
    static void store_biases(void);
    static void store_biases_in_flash(const int32_t acc_bias_q16[3], const int32_t gyr_bias_q16[3],
                                      const int32_t mag_bias_q16[3]);
    static int retrieve_stored_biases_from_flash(int32_t acc_bias_q16[3], int32_t gyr_bias_q16[3],
                                                 int32_t mag_bias_q16[3]);
};

/*
 * ICM mounting matrix
 * Coefficients are coded as Q30 integer
 */
#if defined(ICM_FAMILY_CPLUS)
static int32_t icm_mounting_matrix[9] = {0, -(1 << 30), 0, (1 << 30), 0, 0, 0, 0, (1 << 30)};
#else
static int32_t icm_mounting_matrix[9] = {(1 << 30), 0, 0, 0, (1 << 30), 0, 0, 0, (1 << 30)};
#endif
#endif
